﻿#include "PipelineStack.h"

#include "componentmanager.h"
#include "wcs/WCSSend.h"
#include "stackplate/stackPlate.h"
#include "boxline/boxline.h"
#include "project_data.h"

#include "pipeline_log.h"


PipelineStack::PipelineStack()
{}

PipelineStack::~PipelineStack()
{}

bool PipelineStack::Init()
{
	if (m_info.type != PipelineType::pipeline_stack) {
		log_error("PIpeline type is error, type = {}", (int)m_info.type);
		return false;
	}

	m_wcs = dynamic_cast<WCSSend*>(component->GetComponent(Component::component_wcsSend));
	if (m_wcs == nullptr) {
		log_error("Get wcs send comp failed");
		return false;
	}

	auto plateM = component->GetComponent(Component::component_stackPlate);
	if (plateM == nullptr) {
		log_error("Get stack plateM comp failed");
		return false;
	}
	m_put = static_cast<StackPlate*>(plateM->GetPoint(m_info.putdown));
	if (m_put == nullptr) {
		log_error("Get stack plate [{}] failed", m_info.putdown);
		return false;
	}

	auto lineM = component->GetComponent(Component::component_boxline);
	if (lineM == nullptr) {
		log_error("Get BoxlineM comp failed");
		return false;
	}

	m_grab = static_cast<Boxline*>(lineM->GetPoint(m_info.grab));
	if (m_grab == nullptr) {
		log_error("Get boxlinde [{}] failed", m_info.grab);
		return false;
	}

	return true;
}

bool PipelineStack::CheckTaskData(const QByteArray& task)
{
	TaskDataJ data = {};
	data.fromJson(task);
	if (data.robNo() != m_info.rob) {
		log_error("任务数据机械手编号不匹配，任务：{}， 本机{}", data.robNo(), m_info.rob);
		return false;
	}
	if (data.skuList().size() != 1) {
		return false;
	}
	if (m_info.putdown != data.putNo() || m_info.grab != data.pickNo()) {
		return false;
	}
	auto& sku = data.skuList().at(0);
	if (sku.height() <= 0 || sku.length() <= 0 || sku.width() <= 0) {
		log_error("Sku Size error");
		return false;
	}
	if (data.taskNum() > 2 || data.taskNum() <= 0) {
		log_error("task Num error");
		return false;
	}

	log_trace("{} reciever a task id = {}", m_info.id, data.taskId());
	return true;

}
int PipelineStack::IfTaskAvailable(const QByteArray& task)
{
	TaskDataJ data = {};
	data.fromJson(task);
	if (!CheckTaskData(task)) {
		return -1;
	}
	if (m_task.isBusy) {
		SetTraceMassage(QString("当前正在执行任务 %1，无法接收新任务（%2）").arg(m_task.taskId).arg(data.taskId()));
		log_error("Task matching, but there are unfinished tasks");
		return 1;
	}
	m_task.stackType = 0;
	m_task.taskId = data.taskId();
	m_task.taskNum = data.taskNum();
	auto& sku = data.skuList().at(0);
	m_sku.skuNo = sku.skuNo();
	m_sku.pos = 0;
	m_sku.size.width = sku.width();
	m_sku.size.height = sku.height();
	m_sku.size.length = sku.length();
	m_sku.type = sku.boxType();
	m_sku.weight = sku.weight();
	m_sku.valied = true;
	m_task.isBusy = true;
	m_sku.valied = true;
	SetTraceMassage(QString("收到新任务 %1, 任务数量：%2").arg(m_task.taskId).arg(m_task.taskNum));
	return 0;

}
/****************************************************************************
 * 功能: 任务初始化操作，由项目具体实现
 * 返回值: true：执行成功，业务线切换到任务初始化完成状态
 ****************************************************************************/
#define GAP (5)
bool PipelineStack::TaskInit()
{
	m_waitFinishStep.clear();
	m_execStep.clear();
	m_prepareStep.clear();
	// 托盘物料信息校验
	auto info = m_put->GetPlateStateInfo();
	if (info.sku.valied) {
		auto& size = info.sku.size;
		if (std::abs(size.height - m_sku.size.height) > GAP || std::abs(size.width - m_sku.size.width) > GAP ||
			std::abs(size.length - m_sku.size.length) > GAP) {
			SetPipelineErrorState("任务物料的长宽高和托盘物料不匹配，任务无法执行");
			return false;
		}
	} else {
		m_put->UpdataPlateSkuInfo(m_sku, m_task.stackType);
	}

	// 任务下发表示物料已经到位
	m_grab->SetSkuArrived(m_task.taskNum);
	m_grab->UpdateSkuInfo(m_sku);
	// 生成节拍数据
	m_prepareStep.valied = true;
	m_prepareStep.id = m_stepId++;
	m_prepareStep.excuteNum = m_task.taskNum;
	return true;
}
/****************************************************************************
 * 功能: 判断任务启动条件是否具备
 * 返回值:true：任务可以启动，业务线切换到任务开始状态
 ****************************************************************************/
bool PipelineStack::IfTaskStarted()
{
	if (m_put->GetPlateBusyState() == true) { // 托盘被别的任务占用
		return false;
	}
	// 托盘到位物料到位后
	if (m_put->GetPlateState() && m_grab->GetState() > 0) {
		if (!m_put->SetPlateBusy()) {
			log_error("Set plate  busy failed, bug");
			return false;
		}
		SetTraceMassage("托盘与物料准备完成");
		return true;
	}
	return false;
}

/****************************************************************************
 * 功能: 判断任务是否准备完成
 * 返回值: true: 任务已经准备完成，直接进入任务等待状态
 ****************************************************************************/
bool PipelineStack::IfTaskPrepared()
{
	if (m_task.taskNum - m_exec.finishNum - m_exec.execNum > 0) {
		return false;
	}

	return true;
}

/****************************************************************************
 * 功能: 用于生成新节拍
 * 返回值: 0：节拍生成成功，业务线切换到节拍准备完成状态，1：生成条件不足， -1：生成失败
 ****************************************************************************/
int PipelineStack::StepPrepare()
{
	QString info;
	bool ret = m_put->CalculateNextGrab(m_prepareStep.id, m_prepareStep.excuteNum, info);
	if (!ret) {
		SetPipelineErrorState("放置位置算法计算失败:" + info);
		return -1;
	}
	if (!m_put->GetNextGrabPosition(m_prepareStep.id, m_prepareStep.putPos)) {
		SetPipelineErrorState(QString("获取放置点[%1]坐标失败").arg(m_info.putdown));
		return -1;

	}

	if (!m_grab->CalculateNextGrab(m_prepareStep.id, m_prepareStep.excuteNum, info)) {
		SetPipelineErrorState(QString("抓取点[%1]计算抓取坐标失败：%2").arg(m_info.grab).arg(info));
		return -1;
	}
	if (!m_grab->GetSkuPosition(m_prepareStep.id, m_prepareStep.grabPos)) {
		SetPipelineErrorState(QString("获取抓取点[%1]坐标失败").arg(m_info.grab));
		return -1;
	}
	QString paths = "A-1,A-2,A-3";
	AddPathListToPrepareStepInfo(paths);

	SetTraceMassage(QString("节拍%1准备完成").arg(m_prepareStep.id));
	return 0;
}

/****************************************************************************
 * 功能: 任务完成处理操作， 任务完成状态时被调用
 * 返回值: true 表示成功
 ****************************************************************************/
bool PipelineStack::TaskFinish()
{
	if (!moveInfoSend) {
		if (SendMoveInfo()) {
			SetTraceMassage("重新发送移走通知成功");
		} else {
			SetErrorMassage(QString("重新发送物料移走通知失败"));
			return false;
		}
	}

	SetTraceMassage(QString("任务执行完成，发送任务完成通知"));
	TaskFinishInfo data = {};
	data.robNo = m_info.rob;
	data.taskId = m_task.taskId;
	data.finishNum = m_exec.finishNum;
	data.taskFinish = m_exec.finishNum < m_task.taskNum;

	data.plateId = m_info.putdown;
	QString err;
	if (!m_wcs->SendTaskFinishInfo(data.toByteArray(), err)) {
		log_error("send Task Finish info failed, err = {}", err);
		SetErrorMassage(QString("发送任务完成通知失败，%1").arg(err));
		return false;
	}
	DataInit();
	moveInfoSend = false;
	return true;

}

/*路径执行前操作*/
bool PipelineStack::PathStart(const QString& path)
{
	auto list = path.split("-");
	if (list.size() == 2 && list.at(1) == "2") {
		// 放货路径完成后，将货物数量新增到码垛托盘
		return m_grab->SetSkuMoved();
	}

	return true;
}

#define SKU_MOVE ("/sku/moved")

structJ(SkuMoveInfo,
	mapping(QString, robNo);
mapping(QString, placeNo);
mapping(QString, taskId);
prop(&robNo, &placeNo, &taskId);
)



bool PipelineStack::SendMoveInfo()
{
	SkuMoveInfo info = {};
	info.placeNo = m_info.grab;
	info.robNo = m_info.rob;
	info.taskId = m_task.taskId;
	QByteArray ansData;
	WCSAns ans = {};
	if (!m_wcs->SendToWCS(SKU_MOVE, info.toByteArray(), ansData)) {
		SetErrorMassage("发送移位通知失败:" + ansData);
		moveInfoSend = false;
		return false;
	}
	ans.fromJson(ansData);
	if (ans.code() < 0) {	// 消息返回失败
		SetErrorMassage("发送移位通知失败:" + ans.msg());
		moveInfoSend = false;
		return false;
	}
	moveInfoSend = true;

	SetTraceMassage("移位通知发送成功");
	return true;
}


bool PipelineStack::SendNotifyer(int type)
{
	if (type == 1) { // 发生移位通知
		SendMoveInfo();
		return true;
	}

	return false;
}
/*路径执行后特殊操作*/
bool PipelineStack::PathEnd(const QString& path)
{
	auto list = path.split("-");
	if (list.size() == 2 && list.at(1) == "2") {
		// 放货路径完成后，将货物数量新增到码垛托盘
		if (m_put->AddSkuToPlate(m_waitFinishStep.excuteNum)) {
			return m_put->SetPlateRelease();
		}
		return false;
	}
	return true;
}
